#include "speed_timer.h"

extern volatile ControlMode_t CURR_MODE;
extern volatile float current_target;

volatile float setVelocity = 180; // 设定转速
volatile float velocity; // 实际转速

lowpass_filter_type LPF_Velocity = {0}; // 转速低通滤波
PID_LocTypeDef speed_pid; // 转速环PID

volatile float setAngle = 2 * _PI; // 设定角度
PID_LocTypeDef pos_pid; // 角度环PID

volatile uint8_t angle_cnt = 0;
volatile uint32_t ms_tick = 0;

void speed_timer_init(void)
{
  /* enable tmr1 clock */
  crm_periph_clock_enable(CRM_TMR4_PERIPH_CLOCK, TRUE);

  /* tmr10 configuration */
  /* time base configuration */
  /* systemclock/19200/10000 = 1hz */
  tmr_base_init(TMR4, 9, (200000000 / 10000) - 1);
  tmr_cnt_dir_set(TMR4, TMR_COUNT_UP);

  tmr_flag_clear(TMR4, TMR_OVF_FLAG);
  /* overflow interrupt enable */
  tmr_interrupt_enable(TMR4, TMR_OVF_INT, TRUE);
  nvic_irq_enable(TMR4_GLOBAL_IRQn, 3, 0);

  speed_pid.kp = 0.06;
  speed_pid.ki = 0.0008;
  speed_pid.kd = 0;
  speed_pid.limit = 0.7;
  speed_pid.ka = 1;
  speed_pid.ns = 0;

  pos_pid.kp = 3;
  pos_pid.ki = 0.0;
  pos_pid.kd = 0;
  pos_pid.ka = 1;
  pos_pid.limit = 100;

  LPF_Velocity.sample_freq = 1000;
  LPF_Velocity.bandwidth = 600;
  lowpass_filter_init(&LPF_Velocity);

  /* enable tmr10 */
  tmr_counter_enable(TMR4, TRUE);
}

void TMR4_GLOBAL_IRQHandler(void)
{
  if(tmr_flag_get(TMR4, TMR_OVF_FLAG) != RESET)
  {
    tmr_flag_clear(TMR4, TMR_OVF_FLAG);
    ms_tick++;
    if(CURR_MODE != NOT_STARTED)
    {
      velocity = lowpass_filtering(&LPF_Velocity, encoder_GetVelocity());
    }
    switch(CURR_MODE)
    {
    case VELOCITY:
    {
      current_target = PID_Loc(setVelocity, velocity, &speed_pid);
      break;
    }
    case ANGLE:
    {
      if(++angle_cnt == 5)
      {
        angle_cnt = 0;
        setVelocity = PID_Loc(setAngle, encoder_GetFullAngle(), &pos_pid);
      }
      current_target = PID_Loc(setVelocity, velocity, &speed_pid);
      break;
    }
    default:
      break;
    }
  }
}
